#include <stdint.h>
#include <string.h>  //for memset
#include <stdbool.h> //for bool
#include <assert.h>  //for assert
#include "mem.h"     //for memAlloc
#include "foc.h"
#include "math.h"

#define EANGLE_TO_MANGLE(E, M, P) (E = (M * P))

#define SQRT_3 1.7320508075688772935274463415059
#define _2PI 6.2831853071795864769252867665590
#define PI 3.1415926535897932384626433832795

typedef struct
{
    float Kp; // 比例增益
    float Ki; // 积分增益
    float Kd; // 微分增益

    float integratorLimit; // 积分项的限制，防止积分风暴
    float outputLimit;     // 控制输出的限制

    float prevError; // 上一次误差，用于微分计算
    float integral;  // 当前的积分项
    float dt;        // 控制周期时间间隔（采样时间）

    float targetVal; // 目标值（位置、速度或电流）
    float actualVal; // 实际值（位置、速度或电流）
    float error;     // 当前误差（目标 - 实际）
} PIDController;

/// @brief foc对象的完整数据
typedef struct
{
    // 接口部分
    foc_t foc;

    // 私有部分
    uint8_t polePairs;
    // uint16_t vSupply;
    uint16_t vSupplyHalf;

    float Ualpha, Ubeta;

    float Uq, Ud; // 用于Park变换

    float Ua, Ub, Uc; // 用于clarke变换

    float eAngle, mAngle; // 电角度与机械角度

    // 三环pid控制器
    PIDController posPID;
    PIDController speedPID;
    PIDController currentPID;

    // 编码器相关
    float mechanicalAngle;
    float multiAngle;
    float elAngleOffset;
    float curVel;

    void (*setPwm)(float Ua, float Ub, float Uc);
    bool (*getEncoderData)(void *self, float *mechanicalAngle, float *multiAngle, float *curVel);

} data_t;

// 如果是单例模式，则直接使用：
// static data_t theOne = {0};

static void loop(foc_p self, uint32_t tickNow);

foc_p newFoc(int ID, uint8_t polePairs, uint16_t vSupply, void (*setPwm)(float Ua, float Ub, float Uc))
{
    data_t *this = (data_t *)memAlloc(sizeof(data_t));
    assert(this);

    // todo: init
    this->polePairs = polePairs;
    this->vSupplyHalf = vSupply / 2;

    this->setPwm = setPwm;

    FOC_INIT(this, loop);
    return (foc_p)this;
}

float normalizeAngle(float angle)
{
    float a = fmod(angle, _2PI); // 取余运算可以用于归一化，列出特殊值例子算便知
    return a >= 0 ? a : (a + _2PI);
}

static void rePark(foc_p self, float Uq, float Ud)
{
    data_t *this = (data_t *)self;
    assert(this);

    // this->Ualpha = this->Ud * cosf(this->eAngle) - this->Uq * sinf(this->eAngle);
    // this->Ubeta = this->Ud * sinf(this->eAngle) + this->Uq * cosf(this->eAngle);
    this->Ualpha = Ud * cosf(this->eAngle) - Uq * sinf(this->eAngle);
    this->Ubeta = Ud * sinf(this->eAngle) + Uq * cosf(this->eAngle);
}

static void reClarke(foc_p self)
{
    data_t *this = (data_t *)self;
    assert(this);

    this->Ua = this->Ualpha + this->vSupplyHalf;
    this->Ub = (SQRT_3 * this->Ubeta - this->Ualpha) / 2.0f + this->vSupplyHalf;
    this->Uc = (-SQRT_3 * this->Ubeta - this->Ualpha) / 2.0f + this->vSupplyHalf;
}

static void setPhaseVoltage(foc_p self, float Uq, float Ud)
{
    data_t *this = (data_t *)self;
    assert(this);

    rePark(self, Uq, Ud);

    reClarke(self);

    this->setPwm(this->Ua, this->Ub, this->Uc);
}

void velocityOpenloop(foc_p self, float target_velocity, uint32_t tickNow)
{
    data_t *this = (data_t *)self;
    assert(this);
    static uint32_t timeStamp;

    float Ts = (tickNow - timeStamp) * 1e-3f; // 计算时间间隔，单位为秒

    if (Ts <= 0 || Ts > 0.5f) // 防止时间间隔异常
        Ts = 1e-3f;

    this->mechanicalAngle = normalizeAngle(this->mechanicalAngle + target_velocity * Ts); // 计算机械角度

    EANGLE_TO_MANGLE(this->eAngle, this->mechanicalAngle, this->polePairs);

    setPhaseVoltage(self, 5, 0);
}

static void loop(foc_p self, uint32_t tickNow)
{
    data_t *this = (data_t *)self;
    assert(this);

    velocityOpenloop(self, 5, tickNow);
}

#if 0
static void init(foc_p self, uint32_t param)
{
    data_t *this = (data_t *)self;
    assert(this);
    (void)param;
}

#endif